/*
 Integrating Distance Reader with Distance Displayer
 Distance Reader = Maxbotix Ultrasensor
 Distance Displayer = 16x2 LCD
*/

// using the maxsonar quick start http://www.adafruit.com
// http://www.adafruit.com/index.php?main_page=product_info&cPath=35&products_id=172

#include <LiquidCrystal.h>
// Initialize the library with the numbers of the interface pins 
LiquidCrystal lcd(12, 11, 5, 4, 3, 7); // Create an lcd object and assign the pins
//LiquidCrystal lcd(12, 11, 5, 4, 3, 2); // Create an lcd object and assign the pins

//DISTANCE SENSOR
int sonarPin = 0; //pin connected to analog out on maxsonar sensor
int inchesAway;   // distance
int timeDelay = 500; // delay
int alert;

//SOUND
unsigned long echo = 0;
int ultraSoundSignal = 9;
unsigned long ultrasoundValue = 0;
boolean WELCOME = true;
int speakerOut = 8;
// Do we want debugging on serial out? 1 for yes, 0 for no
int DEBUG = 0;

// MELODY and TIMING  =======================================
//  melody[] is an array of notes, accompanied by beats[], 
//  which sets each note's relative length (higher #, longer note) 
//int melody[] = {  C,  b,  g,  C,  b,   e,  R,  C,  c,  g, a, C };
//int melody[] = {  956,  1014,  1275,  956,  1014,   1519,  0,  956,  1915,  1275, 1136, 956 };
//int beats[]  = { 16, 16, 16,  8,  8,  16, 32, 16, 16, 16, 8, 8 }; 
int melody[] = { 956,  1915};//,  1275, 1136, 956, 0}; //,  956,  1915,  1275, 1136, 956 };
int beats[]  = { 16,   16};//,    16,   8,    8 , 32};//, 16, 16, 16, 8, 8 }; 
int melody3[] = {  956,  0};//, 1275, 0,  956, 0, 1275, 0, 956, 0, 1275, 0   };
int beats3[]  = { 16, 32};//, 16, 8,  16,  8,  16, 8, 16, 8, 16, 8 }; 
int melody2[] = {  1275,  956};//, 1275,  956, 1275,   956,  0};//,   1275,  956, 1275, 956,  1275,0 };
int beats2[]  = { 4,      4};//,   4,     4,   4,      4,    4}; //, 4, 4, 4, 4, 4, 16 };

int melody1[] = {  1275,  956};//, 1275,  956, 1275,   956,  0};//,   1275,  956, 1275, 956,  1275,0 };
int beats1[]  = {  2,     2};//,   2,     2,   2,      2,    4};//, 2, 2, 2, 2, 2, 16 }; 

int MAX_COUNT = sizeof(melody) / 2; // Melody length, for looping.

// Set overall tempo
//long tempo = 10000;
long tempo = 50000;
// Set length of pause between notes
int pause = 1000;
int pause2 = 100;
// Loop variable to increase Rest length
int rest_count = 100; //<-BLETCHEROUS HACK; See NOTES

// Initialize core variables
int tone_ = 0;
int beat = 0;
long duration  = 0;


void setup() {
  Serial.begin(9600);
  pinMode(speakerOut, OUTPUT);
  if (DEBUG) { 
    Serial.begin(9600); // Set serial out if we want debugging
  } 
  lcd.begin(16, 2); // Set the display to 16 columns and 2 rows
}

void loop() {
  inchesAway = analogRead(sonarPin) /2; // read analog
  setCursorDemo(inchesAway);
  //delay(timeDelay); // delay
  Serial.println(inchesAway); // print value
  if (WELCOME) {
       playWelcomeMelody();
       WELCOME = false;
    }
  
    alert = getSafetyLevel();        
    if (alert == 3) {
      playLevelThreeAlert();
    }

    alert = getSafetyLevel(); //getAlertLevel(x);
    if (alert==2) {
      playLevelTwoAlert();                
    }      
  
    if (alert==1) { 
      playLevelOneAlert();  
    }
}

void setCursorDemo(int distance) { 
	lcd.clear(); // Clear the display 
	lcd.print("Distance (in)"); // Print some text 
	lcd.setCursor(1,0); // Cursor at column 5 row 0  
	lcd.setCursor(10,1); // Cursor at column 10 row 1 
        lcd.print(distance);// +" inches"); 

	lcd.setCursor(3,1); // Cursor at column 3 row 1 
	lcd.print("3,1");  
}

// PLAY TONE  ==============================================
// Pulse the speaker to play a tone for a particular duration
void playTone() {
  long elapsed_time = 0;
  if (tone_ > 0) { // if this isn't a Rest beat, while the tone has 
    //  played less long than 'duration', pulse speaker HIGH and LOW
    while (elapsed_time < duration) {

      digitalWrite(speakerOut,HIGH);
      delayMicroseconds(tone_ / 2);

      // DOWN
      digitalWrite(speakerOut, LOW);
      delayMicroseconds(tone_ / 2);

      // Keep track of how long we pulsed
      elapsed_time += (tone_);
    } 
  }
  else { // Rest beat; loop times delay
    for (int j = 0; j < rest_count; j++) { // See NOTE on rest_count
      delayMicroseconds(duration/2);  
    }                                
  }                                 
}

int getSafetyLevel() {
  int safetyLevel = 3;
  inchesAway = analogRead(sonarPin) /2;
  Serial.println(inchesAway);

  if (inchesAway < 16 && WELCOME == false ) { // if something is 24 inches away then make a 1khz sound
    safetyLevel = 1;    
  } else if (inchesAway <25) {
    safetyLevel = 2;   
  } 
  return safetyLevel;
}

void playWelcomeMelody() {
  // Welcome Melody
        for (int i=0; i<MAX_COUNT; i++) {
          tone_ = melody[i];
          beat = beats[i];

          duration = beat * tempo; // Set up timing

          playTone(); 
          // A pause between notes...
          //delayMicroseconds(pause);

          if (DEBUG) { // If debugging, report loop, tone, beat, and duration
            Serial.print(i);
            Serial.print(":");
            Serial.print(beat);
            Serial.print(" ");    
            Serial.print(tone_);
            Serial.print(" ");
            Serial.println(duration);
          }
        }
}
void playLevelThreeAlert() {
  
  if (alert == 3) {
         //Safe sound
        // Set up a counter to pull from melody[] and beats[]
        for (int i=0; i<MAX_COUNT; i++) {
          tone_ = melody3[i];
          beat = beats3[i];

          duration = beat * tempo; // Set up timing

          playTone(); 
          // A pause between notes...
          // delayMicroseconds(pause);

          if (DEBUG) { // If debugging, report loop, tone, beat, and duration
              Serial.print(i);
              Serial.print(":");
              Serial.print(beat);
              Serial.print(" ");    
              Serial.print(tone_);
              Serial.print(" ");
              Serial.println(duration);
          }
        }
    }

}

void playLevelTwoAlert() {
  // Set up a counter to pull from melody[] and beats[]
        for (int i=0; i<MAX_COUNT; i++) {
              tone_ = melody2[i];
              beat = beats2[i];

              duration = beat * tempo; // Set up timing

              playTone(); 
              // A pause between notes...
              // delayMicroseconds(pause2);

             if (DEBUG) { // If debugging, report loop, tone, beat, and duration
                  Serial.print(i);
                  Serial.print(":");
                  Serial.print(beat);
                  Serial.print(" ");    
                  Serial.print(tone_);
                  Serial.print(" ");
                  Serial.println(duration);
             }
         }
}

void playLevelOneAlert() {
  //Unsafe
  // Set up a counter to pull from melody[] and beats[]
  for (int i=0; i<MAX_COUNT; i++) {
    tone_ = melody1[i];
    beat = beats1[i];

    duration = beat * tempo; // Set up timing

    playTone(); 
    // A pause between notes...
    //delayMicroseconds(pause2);

    if (DEBUG) { // If debugging, report loop, tone, beat, and duration
      Serial.print(i);
      Serial.print(":");
      Serial.print(beat);
      Serial.print(" ");    
      Serial.print(tone_);
      Serial.print(" ");
      Serial.println(duration);
    }
  }
}
